
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_imu_backend.c
  * @author     baiyang
  * @date       2021-10-11
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <string.h>
#include "sensor_imu.h"
#include "sensor_imu_backend.h"

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
void imu_backend_destructor(sensor_imu_backend *imu_backend);
bool imu_backend_update(sensor_imu_backend *imu_backend);
void imu_backend_accumulate(sensor_imu_backend *imu_backend);
void imu_backend_start(sensor_imu_backend *imu_backend);
bool imu_backend_get_output_banner(sensor_imu_backend *imu_backend, char* banner, uint8_t banner_len);
/*----------------------------------variable----------------------------------*/
// access to frontend
static sensor_imu *_imu_p;

struct sensor_imu_backend_ops imu_backend_ops = {.sensor_imu_backend_destructor = imu_backend_destructor,
                                                        .update = imu_backend_update,
                                                        .accumulate = imu_backend_accumulate,
                                                        .start = imu_backend_start,
                                                        .get_output_banner = imu_backend_get_output_banner};

static uitc_sensor_gyr sensor_gyr[INS_MAX_INSTANCES];
static uitc_sensor_acc sensor_acc[INS_MAX_INSTANCES];
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void imu_backend_destructor(sensor_imu_backend *imu_backend) { rt_sem_delete(imu_backend->_sem); }
bool imu_backend_update(sensor_imu_backend *imu_backend) { return false; }
void imu_backend_accumulate(sensor_imu_backend *imu_backend) {}
void imu_backend_start(sensor_imu_backend *imu_backend) {}
bool imu_backend_get_output_banner(sensor_imu_backend *imu_backend, char* banner, uint8_t banner_len) { return false; }

/**
  * @brief       
  * @param[in]   imu_backend  
  * @param[in]   imu  
  * @param[out]  
  * @retval      
  * @note        
  */
void sensor_imu_backend_ctor(sensor_imu_backend *imu_backend, char *name)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的指针变量初始为非零值。
    memset(imu_backend, 0, sizeof(sensor_imu_backend));

    _imu_p = sensor_imu_get_singleton();

    imu_backend->ops = &imu_backend_ops;

    imu_backend->_clip_limit = 15.5f * GRAVITY_MSS;
    imu_backend->_id         = -1;

    imu_backend->_sem = rt_sem_create(name, 0, RT_IPC_FLAG_FIFO);
}

// indicate the backend is doing sensor-rate sampling for this accel
void sensor_imu_backend_set_accel_sensor_rate_sampling_enabled(sensor_imu_backend *imu_backend, uint8_t instance, bool value)
{
    const uint8_t bit = (1<<instance);
    if (value) {
        _imu_p->_accel_sensor_rate_sampling_enabled |= bit;
    } else {
        _imu_p->_accel_sensor_rate_sampling_enabled &= ~bit;
    }
}

void sensor_imu_backend_set_gyro_sensor_rate_sampling_enabled(sensor_imu_backend *imu_backend, uint8_t instance, bool value)
{
    const uint8_t bit = (1<<instance);
    if (value) {
        _imu_p->_gyro_sensor_rate_sampling_enabled |= bit;
    } else {
        _imu_p->_gyro_sensor_rate_sampling_enabled &= ~bit;
    }
}

void sensor_imu_backend_set_gyro_orientation(uint8_t instance, enum RotationEnum rotation)
{
    _imu_p->_gyro_orientation[instance] = rotation;
}

void sensor_imu_backend_set_accel_orientation(uint8_t instance, enum RotationEnum rotation)
{
    _imu_p->_accel_orientation[instance] = rotation;
}

// increment clipping counted. Used by drivers that do decimation before supplying
// samples to the frontend
void sensor_imu_backend_increment_clip_count(uint8_t instance)
{
    _imu_p->_accel_clip_count[instance]++;
}

// should fast sampling be enabled on this IMU?
bool sensor_imu_backend_enable_fast_sampling(uint8_t instance)
{
    return (_imu_p->_fast_sampling_mask & (1U<<instance)) != 0;
}

// if fast sampling is enabled, the rate to use in kHz
uint8_t sensor_imu_backend_get_fast_sampling_rate()
{
    return (1 << (uint8_t)_imu_p->_fast_sampling_rate);
}

/*
  notify of a FIFO reset so we don't use bad data to update observed sensor rate
 */
void sensor_imu_backend_notify_accel_fifo_reset(uint8_t instance)
{
    _imu_p->_sample_accel_count[instance] = 0;
    _imu_p->_sample_accel_start_us[instance] = 0;
}

/*
  notify of a FIFO reset so we don't use bad data to update observed sensor rate
 */
void sensor_imu_backend_notify_gyro_fifo_reset(uint8_t instance)
{
    _imu_p->_sample_gyro_count[instance] = 0;
    _imu_p->_sample_gyro_start_us[instance] = 0;
}

// set the amount of oversamping a accel is doing
void sensor_imu_backend_set_accel_oversampling(sensor_imu_backend *imu_backend, uint8_t instance, uint8_t n)
{
    _imu_p->_accel_over_sampling[instance] = n;
}

// set the amount of oversamping a gyro is doing
void sensor_imu_backend_set_gyro_oversampling(sensor_imu_backend *imu_backend, uint8_t instance, uint8_t n)
{
    _imu_p->_gyro_over_sampling[instance] = n;
}

/*
  update the sensor rate for FIFO sensors

  FIFO sensors produce samples at a fixed rate, but the clock in the
  sensor may vary slightly from the system clock. This slowly adjusts
  the rate to the observed rate
*/
void sensor_imu_backend_update_sensor_rate(sensor_imu_backend *imu_backend, uint16_t *count, uint32_t *start_us, float *rate_hz)
{
    uint32_t now = time_micros();
    if ((*start_us) == 0) {
        *count = 0;
        *start_us = now;
    } else {
        (*count)++;
        if (now - (*start_us) > 1000000UL) {
            float observed_rate_hz = (*count) * 1.0e6f / (now - *start_us);
#if 0
            printf("IMU RATE: %.1f should be %.1f\n", observed_rate_hz, rate_hz);
#endif
            float filter_constant = 0.98f;
            float upper_limit = 1.05f;
            float lower_limit = 0.95f;
            if (sensor_imu_backend_sensors_converging()) {
                // converge quickly for first 30s, then more slowly
                filter_constant = 0.8f;
                upper_limit = 2.0f;
                lower_limit = 0.5f;
            }
            observed_rate_hz = math_constrain_float(observed_rate_hz, (*rate_hz)*lower_limit, (*rate_hz)*upper_limit);
            *rate_hz = filter_constant * (*rate_hz) + (1-filter_constant) * observed_rate_hz;
            *count = 0;
            *start_us = now;
        }
    }
}

void sensor_imu_backend_rotate_and_correct_accel(sensor_imu_backend *imu_backend, uint8_t instance, Vector3f_t *accel) 
{
    /*
      accel calibration is always done in sensor frame with this
      version of the code. That means we apply the rotation after the
      offsets and scaling.
     */

    // rotate for sensor orientation
    vec3_rotate(accel, _imu_p->_accel_orientation[instance]);

    sensor_acc[instance].sensor_acc_measure[0] = accel->x;
    sensor_acc[instance].sensor_acc_measure[1] = accel->y;
    sensor_acc[instance].sensor_acc_measure[2] = accel->z;

    // TOOD:后期增加速度计温度补偿
    // 

#if HAL_INS_TEMPERATURE_CAL_ENABLE
    // apply temperature corrections
    //_imu_p->tcal[instance].correct_accel(_imu_p->get_temperature(instance), _imu_p->caltemp_accel[instance], accel);
#endif

    // apply offsets
    vec3_sub(accel, accel, &(_imu_p->_accel_offset[instance]));

    // apply scaling
    const Vector3f_t *accel_scale = &(_imu_p->_accel_scale[instance]);
    accel->x *= accel_scale->x;
    accel->y *= accel_scale->y;
    accel->z *= accel_scale->z;

    // rotate to body frame
    if (_imu_p->_board_orientation == ROTATION_CUSTOM) {
        float temp[3];
        temp[0] = accel->x;
        temp[1] = accel->y;
        temp[2] = accel->z;

        accel->vec3[0] = _imu_p->_custom_rotation[0][0] * temp[0] + _imu_p->_custom_rotation[0][1] * temp[1] + _imu_p->_custom_rotation[0][2] * temp[2];
        accel->vec3[1] = _imu_p->_custom_rotation[1][0] * temp[0] + _imu_p->_custom_rotation[1][1] * temp[1] + _imu_p->_custom_rotation[1][2] * temp[2];
        accel->vec3[2] = _imu_p->_custom_rotation[2][0] * temp[0] + _imu_p->_custom_rotation[2][1] * temp[1] + _imu_p->_custom_rotation[2][2] * temp[2];
    } else {
        vec3_rotate(accel, _imu_p->_board_orientation);
    }

    sensor_acc[instance].sensor_acc_correct[0] = accel->x;
    sensor_acc[instance].sensor_acc_correct[1] = accel->y;
    sensor_acc[instance].sensor_acc_correct[2] = accel->z;
}

void sensor_imu_backend_rotate_and_correct_gyro(sensor_imu_backend *imu_backend, uint8_t instance, Vector3f_t *gyro) 
{
    /*
      accel calibration is always done in sensor frame with this
      version of the code. That means we apply the rotation after the
      offsets and scaling.
     */

    // rotate for sensor orientation
    vec3_rotate(gyro, _imu_p->_gyro_orientation[instance]);

    sensor_gyr[instance].sensor_gyr_measure[0] = gyro->x;
    sensor_gyr[instance].sensor_gyr_measure[1] = gyro->y;
    sensor_gyr[instance].sensor_gyr_measure[2] = gyro->z;

    // TOOD:后期增加速度计温度补偿
    // 

#if HAL_INS_TEMPERATURE_CAL_ENABLE
    // apply temperature corrections
    _imu_p->tcal[instance].correct_gyro(_imu_p->get_temperature(instance), _imu_p->caltemp_gyro[instance], gyro);
#endif

    // apply offsets
    vec3_sub(gyro, gyro, &(_imu_p->_gyro_offset[instance]));

    // rotate to body frame
    if (_imu_p->_board_orientation == ROTATION_CUSTOM) {
        float temp[3];
        temp[0] = gyro->x;
        temp[1] = gyro->y;
        temp[2] = gyro->z;

        gyro->vec3[0] = _imu_p->_custom_rotation[0][0] * temp[0] + _imu_p->_custom_rotation[0][1] * temp[1] + _imu_p->_custom_rotation[0][2] * temp[2];
        gyro->vec3[1] = _imu_p->_custom_rotation[1][0] * temp[0] + _imu_p->_custom_rotation[1][1] * temp[1] + _imu_p->_custom_rotation[1][2] * temp[2];
        gyro->vec3[2] = _imu_p->_custom_rotation[2][0] * temp[0] + _imu_p->_custom_rotation[2][1] * temp[1] + _imu_p->_custom_rotation[2][2] * temp[2];
    } else {
        vec3_rotate(gyro, _imu_p->_board_orientation);
    }

    sensor_gyr[instance].sensor_gyr_correct[0] = gyro->x;
    sensor_gyr[instance].sensor_gyr_correct[1] = gyro->y;
    sensor_gyr[instance].sensor_gyr_correct[2] = gyro->z;
}

/*
  rotate gyro vector and add the gyro offset
 */
void sensor_imu_backend_publish_gyro(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *gyro)
{
    if ((1U<<instance) & _imu_p->imu_kill_mask) {
        return;
    }
    _imu_p->_gyro[instance] = (*gyro);
    _imu_p->_gyro_healthy[instance] = true;

    // publish delta angle
    _imu_p->_delta_angle[instance]    = _imu_p->_delta_angle_acc[instance];
    _imu_p->_delta_angle_dt[instance] = _imu_p->_delta_angle_acc_dt[instance];
    _imu_p->_delta_angle_valid[instance] = true;

    vec3_zero(&(_imu_p->_delta_angle_acc[instance]));
    _imu_p->_delta_angle_acc_dt[instance] = 0;
}

void sensor_imu_backend_notify_new_gyro_raw_sample(sensor_imu_backend *imu_backend,
                                                                   uint8_t instance, const Vector3f_t *gyro,
                                                                   uint64_t sample_us)
{
    if ((1U<<instance) & _imu_p->imu_kill_mask) {
        return;
    }
    float dt;

    sensor_imu_backend_update_sensor_rate(imu_backend, &(_imu_p->_sample_gyro_count[instance]), &(_imu_p->_sample_gyro_start_us[instance]),
                        &(_imu_p->_gyro_raw_sample_rates[instance]));

    uint64_t last_sample_us = _imu_p->_gyro_last_sample_us[instance];

    /*
      we have two classes of sensors. FIFO based sensors produce data
      at a very predictable overall rate, but the data comes in
      bunches, so we use the provided sample rate for deltaT. Non-FIFO
      sensors don't bunch up samples, but also tend to vary in actual
      rate, so we use the provided sample_us to get the deltaT. The
      difference between the two is whether sample_us is provided.
     */
    if (sample_us != 0 && _imu_p->_gyro_last_sample_us[instance] != 0) {
        dt = (sample_us - _imu_p->_gyro_last_sample_us[instance]) * 1.0e-6f;
        _imu_p->_gyro_last_sample_us[instance] = sample_us;
    } else {
        // don't accept below 40Hz
        if (_imu_p->_gyro_raw_sample_rates[instance] < 40) {
            return;
        }

        dt = 1.0f / _imu_p->_gyro_raw_sample_rates[instance];
        _imu_p->_gyro_last_sample_us[instance] = time_micros64();
        sample_us = _imu_p->_gyro_last_sample_us[instance];
    }

    Vector3f_t delta_angle;

    // compute delta angle
    vec3_add(&delta_angle, gyro, &(_imu_p->_last_raw_gyro[instance]));
    vec3_mult(&delta_angle, &delta_angle, 0.5f * dt);

    // compute coning correction
    // see page 26 of:
    // Tian et al (2010) Three-loop Integration of GPS and Strapdown INS with Coning and Sculling Compensation
    // Available: http://www.sage.unsw.edu.au/snap/publications/tian_etal2010b.pdf
    // see also examples/coning.py
    //Vector3f_t delta_coning = (_imu_p->_delta_angle_acc[instance] +
    //                         _imu_p->_last_delta_angle[instance] * (1.0f / 6.0f));
    Vector3f_t delta_coning;
    Vector3f_t last_delta_angle_scalar;

    vec3_mult(&last_delta_angle_scalar, &(_imu_p->_last_delta_angle[instance]), 1.0f / 6.0f);
    vec3_add(&delta_coning, &(_imu_p->_delta_angle_acc[instance]), &last_delta_angle_scalar);

    // delta_coning = delta_coning x delta_angle
    vec3_cross(&delta_coning, &delta_coning, &delta_angle);
    vec3_mult(&delta_coning, &delta_coning, 0.5f);

    rt_sem_take(imu_backend->_sem, RT_WAITING_FOREVER);

    uint64_t now = time_micros64();

    if (now - last_sample_us > 100000U) {
        // zero accumulator if sensor was unhealthy for 0.1s
        vec3_zero(&(_imu_p->_delta_angle_acc[instance]));
        _imu_p->_delta_angle_acc_dt[instance] = 0;
        dt = 0;
        vec3_zero(&delta_angle);
    }

    // integrate delta angle accumulator
    // the angles and coning corrections are accumulated separately in the
    // referenced paper, but in simulation little difference was found between
    // integrating together and integrating separately (see examples/coning.py)
    //_imu_p->_delta_angle_acc[instance] += delta_angle + delta_coning;
    vec3_add2(&(_imu_p->_delta_angle_acc[instance]), &(_imu_p->_delta_angle_acc[instance]), &delta_angle, &delta_coning);
    _imu_p->_delta_angle_acc_dt[instance] += dt;

    // save previous delta angle for coning correction
    _imu_p->_last_delta_angle[instance] = delta_angle;
    _imu_p->_last_raw_gyro[instance] = (*gyro);

    Vector3f_t gyro_filtered = (*gyro);

#if 0
    // apply the notch filter
    if (_gyro_notch_enabled()) {
        gyro_filtered = _imu_p->_gyro_notch_filter[instance].apply(gyro_filtered);
    }

    // apply the harmonic notch filter
    if (gyro_harmonic_notch_enabled()) {
        gyro_filtered = _imu_p->_gyro_harmonic_notch_filter[instance].apply(gyro_filtered);
    }
#endif

    // apply the low pass filter last to attentuate any notch induced noise
    gyro_filtered = lpf_apply1_vec3f(&(_imu_p->_gyro_filter[instance]), &gyro_filtered);

    // if the filtering failed in any way then reset the filters and keep the old value
    if (vec3_is_nan(&gyro_filtered) || vec3_is_inf(&gyro_filtered)) {
        lpf_reset2_vec3f(&(_imu_p->_gyro_filter[instance]));
        //_imu_p->_gyro_notch_filter[instance].reset();
        //_imu_p->_gyro_harmonic_notch_filter[instance].reset();
    } else {
        _imu_p->_gyro_filtered[instance] = gyro_filtered;
    }

    _imu_p->_new_gyro_data[instance] = true;

    sensor_gyr[instance].timestamp_us = _imu_p->_gyro_last_sample_us[instance];
    sensor_gyr[instance].sensor_gyr_filter[0] = gyro_filtered.x;
    sensor_gyr[instance].sensor_gyr_filter[1] = gyro_filtered.y;
    sensor_gyr[instance].sensor_gyr_filter[2] = gyro_filtered.z;

    rt_sem_release(imu_backend->_sem);

    if (instance == 0) {
        itc_publish(ITC_ID(sensor_gyr), &(sensor_gyr[instance]));
    }

}

/*
 rotate accel vector, scale and add the accel offset
*/
void sensor_imu_backend_publish_accel(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *accel)
{
    if ((1U<<instance) & _imu_p->imu_kill_mask) {
       return;
    }
    _imu_p->_accel[instance] = (*accel);
    _imu_p->_accel_healthy[instance] = true;

    // publish delta velocity
    _imu_p->_delta_velocity[instance] = _imu_p->_delta_velocity_acc[instance];
    _imu_p->_delta_velocity_dt[instance] = _imu_p->_delta_velocity_acc_dt[instance];
    _imu_p->_delta_velocity_valid[instance] = true;

    vec3_zero(&(_imu_p->_delta_velocity_acc[instance]));
    _imu_p->_delta_velocity_acc_dt[instance] = 0;
}

void sensor_imu_backend_notify_new_accel_raw_sample(sensor_imu_backend *imu_backend,
                                                                    uint8_t instance, const Vector3f_t *accel,
                                                                    uint64_t sample_us,
                                                                    bool fsync_set)
{
    if ((1U<<instance) & _imu_p->imu_kill_mask) {
        return;
    }
    float dt;

    sensor_imu_backend_update_sensor_rate(imu_backend, &(_imu_p->_sample_accel_count[instance]), &(_imu_p->_sample_accel_start_us[instance]),
                        &(_imu_p->_accel_raw_sample_rates[instance]));

    uint64_t last_sample_us = _imu_p->_accel_last_sample_us[instance];

    /*
      we have two classes of sensors. FIFO based sensors produce data
      at a very predictable overall rate, but the data comes in
      bunches, so we use the provided sample rate for deltaT. Non-FIFO
      sensors don't bunch up samples, but also tend to vary in actual
      rate, so we use the provided sample_us to get the deltaT. The
      difference between the two is whether sample_us is provided.
     */
    if (sample_us != 0 && _imu_p->_accel_last_sample_us[instance] != 0) {
        dt = (sample_us - _imu_p->_accel_last_sample_us[instance]) * 1.0e-6f;
        _imu_p->_accel_last_sample_us[instance] = sample_us;
    } else {
        // don't accept below 40Hz
        if (_imu_p->_accel_raw_sample_rates[instance] < 40) {
            return;
        }

        dt = 1.0f / _imu_p->_accel_raw_sample_rates[instance];
        _imu_p->_accel_last_sample_us[instance] = time_micros64();
        sample_us = _imu_p->_accel_last_sample_us[instance];
    }

    sensor_imu_calc_vibration_and_clipping(instance, accel, dt);

    rt_sem_take(imu_backend->_sem, RT_WAITING_FOREVER);

    uint64_t now = time_micros64();

    if (now - last_sample_us > 100000U) {
        // zero accumulator if sensor was unhealthy for 0.1s
        vec3_zero(&_imu_p->_delta_velocity_acc[instance]);
        _imu_p->_delta_velocity_acc_dt[instance] = 0;
        dt = 0;
    }
    
    // delta velocity
    Vector3f_t delta_velocity;

    vec3_mult(&delta_velocity, accel, dt);
    vec3_add(&(_imu_p->_delta_velocity_acc[instance]), &(_imu_p->_delta_velocity_acc[instance]), &delta_velocity);

    _imu_p->_delta_velocity_acc_dt[instance] += dt;

    _imu_p->_accel_filtered[instance] = lpf_apply1_vec3f(&(_imu_p->_accel_filter[instance]), accel);
    if (vec3_is_nan(&(_imu_p->_accel_filtered[instance])) || vec3_is_inf(&(_imu_p->_accel_filtered[instance]))) {
        lpf_reset2_vec3f(&(_imu_p->_accel_filter[instance]));
    }

    sensor_imu_set_accel_peak_hold(instance, &(_imu_p->_accel_filtered[instance]));

    _imu_p->_new_accel_data[instance] = true;

    sensor_acc[instance].timestamp_us = _imu_p->_accel_last_sample_us[instance];
    sensor_acc[instance].sensor_acc_filter[0] = _imu_p->_accel_filtered[instance].x;
    sensor_acc[instance].sensor_acc_filter[1] = _imu_p->_accel_filtered[instance].y;
    sensor_acc[instance].sensor_acc_filter[2] = _imu_p->_accel_filtered[instance].z;

    rt_sem_release(imu_backend->_sem);

    if (instance == 0) {
        itc_publish(ITC_ID(sensor_acc), &(sensor_acc[instance]));
    }

    // 有可能需要将加速度传感器数据记录到日志中？
}

void sensor_imu_backend_set_accel_max_abs_offset(sensor_imu_backend *imu_backend, uint8_t instance, float max_offset)
{
    _imu_p->_accel_max_abs_offsets[instance] = max_offset;
}

// set accelerometer error_count
void sensor_imu_backend_set_accel_error_count(sensor_imu_backend *imu_backend, uint8_t instance, uint32_t error_count)
{
    _imu_p->_accel_error_count[instance] = error_count;
}

// set gyro error_count
void sensor_imu_backend_set_gyro_error_count(sensor_imu_backend *imu_backend, uint8_t instance, uint32_t error_count)
{
    _imu_p->_gyro_error_count[instance] = error_count;
}

// increment accelerometer error_count
void sensor_imu_backend_inc_accel_error_count(sensor_imu_backend *imu_backend, uint8_t instance)
{
    _imu_p->_accel_error_count[instance]++;
}

// increment gyro error_count
void sensor_imu_backend_inc_gyro_error_count(sensor_imu_backend *imu_backend, uint8_t instance)
{
    _imu_p->_gyro_error_count[instance]++;
}

// return the requested loop rate at which samples will be made available in Hz
uint16_t sensor_imu_backend_get_loop_rate_hz(sensor_imu_backend *imu_backend)
{
    // enum can be directly cast to Hz
    return (uint16_t)_imu_p->_loop_rate;
}

/*
  publish a temperature value for an instance
 */
void sensor_imu_backend_publish_temperature(sensor_imu_backend *imu_backend, uint8_t instance, float temperature)
{
    if ((1U<<instance) & _imu_p->imu_kill_mask) {
        return;
    }

    _imu_p->_temperature[instance] = temperature;
}

/*
  common gyro update function for all backends
 */
void sensor_imu_backend_update_gyro(sensor_imu_backend *imu_backend, uint8_t instance)
{
    rt_sem_take(imu_backend->_sem, RT_WAITING_FOREVER);

    if ((1U<<instance) & _imu_p->imu_kill_mask) {
        rt_sem_release(imu_backend->_sem);
        return;
    }
    if (_imu_p->_new_gyro_data[instance]) {
        sensor_imu_backend_publish_gyro(imu_backend, instance, &(_imu_p->_gyro_filtered[instance]));
        _imu_p->_new_gyro_data[instance] = false;
    }

    // possibly update filter frequency
    if (imu_backend->_last_gyro_filter_hz != _imu_p->_gyro_filter_cutoff || sensor_imu_backend_sensors_converging()) {
        lpf_set_cutoff2_vec3f(&(_imu_p->_gyro_filter[instance]), _imu_p->_gyro_filter_cutoff, 1.0f/_imu_p->_gyro_raw_sample_rates[instance]);
        imu_backend->_last_gyro_filter_hz = _imu_p->_gyro_filter_cutoff;
    }

#if 0
    // possily update the harmonic notch filter parameters
    if (!math_flt_equal(_last_harmonic_notch_bandwidth_hz, gyro_harmonic_notch_bandwidth_hz()) ||
        !is_equal(_last_harmonic_notch_attenuation_dB, gyro_harmonic_notch_attenuation_dB()) ||
        sensors_converging()) {
        _imu_p->_gyro_harmonic_notch_filter[instance].init(_gyro_raw_sample_rate(instance), gyro_harmonic_notch_center_freq_hz(), gyro_harmonic_notch_bandwidth_hz(), gyro_harmonic_notch_attenuation_dB());
        _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
        _last_harmonic_notch_bandwidth_hz = gyro_harmonic_notch_bandwidth_hz();
        _last_harmonic_notch_attenuation_dB = gyro_harmonic_notch_attenuation_dB();
    } else if (!math_flt_equal(_last_harmonic_notch_center_freq_hz, gyro_harmonic_notch_center_freq_hz())) {
        if (num_gyro_harmonic_notch_center_frequencies() > 1) {
            _imu_p->_gyro_harmonic_notch_filter[instance].update(num_gyro_harmonic_notch_center_frequencies(), gyro_harmonic_notch_center_frequencies_hz());
        } else {
            _imu_p->_gyro_harmonic_notch_filter[instance].update(gyro_harmonic_notch_center_freq_hz());
        }
        _last_harmonic_notch_center_freq_hz = gyro_harmonic_notch_center_freq_hz();
    }
    // possily update the notch filter parameters
    if (!math_flt_equal(_last_notch_center_freq_hz, _gyro_notch_center_freq_hz()) ||
        !math_flt_equal(_last_notch_bandwidth_hz, _gyro_notch_bandwidth_hz()) ||
        !math_flt_equal(_last_notch_attenuation_dB, _gyro_notch_attenuation_dB()) ||
        sensors_converging()) {
        _imu_p->_gyro_notch_filter[instance].init(_gyro_raw_sample_rate(instance), _gyro_notch_center_freq_hz(), _gyro_notch_bandwidth_hz(), _gyro_notch_attenuation_dB());
        _last_notch_center_freq_hz = _gyro_notch_center_freq_hz();
        _last_notch_bandwidth_hz = _gyro_notch_bandwidth_hz();
        _last_notch_attenuation_dB = _gyro_notch_attenuation_dB();
    }
#endif

    rt_sem_release(imu_backend->_sem);
}

/*
  common accel update function for all backends
 */
void sensor_imu_backend_update_accel(sensor_imu_backend *imu_backend, uint8_t instance)
{
    rt_sem_take(imu_backend->_sem, RT_WAITING_FOREVER);

    if ((1U<<instance) & _imu_p->imu_kill_mask) {
        rt_sem_release(imu_backend->_sem);
        return;
    }

    if (_imu_p->_new_accel_data[instance]) {
        sensor_imu_backend_publish_accel(imu_backend, instance, &(_imu_p->_accel_filtered[instance]));
        _imu_p->_new_accel_data[instance] = false;
    }
    
    // possibly update filter frequency
    if (imu_backend->_last_accel_filter_hz != _imu_p->_accel_filter_cutoff) {
        lpf_set_cutoff2_vec3f(&(_imu_p->_accel_filter[instance]), _imu_p->_accel_filter_cutoff, 1.0f/_imu_p->_accel_raw_sample_rates[instance]);
        imu_backend->_last_accel_filter_hz = _imu_p->_accel_filter_cutoff;
    }

    rt_sem_release(imu_backend->_sem);
}

// get accelerometer raw sample rate.
float sensor_imu_backend_accel_raw_sample_rate(uint8_t instance)
{
    return _imu_p->_accel_raw_sample_rates[instance];
}

// set accelerometer raw sample rate;  note that the storage type
// is actually float!
void sensor_imu_backend_set_accel_raw_sample_rate(uint8_t instance, uint16_t rate_hz)
{
    _imu_p->_accel_raw_sample_rates[instance] = rate_hz;
}

// get gyroscope raw sample rate
float sensor_imu_backend_gyro_raw_sample_rate(uint8_t instance)
{
    return _imu_p->_gyro_raw_sample_rates[instance];
}

// set gyro raw sample rate; note that the storage type is
// actually float!
void sensor_imu_backend_set_gyro_raw_sample_rate(uint8_t instance, uint16_t rate_hz)
{
    _imu_p->_gyro_raw_sample_rates[instance] = rate_hz;
}

void sensor_imu_backend_set_raw_sample_accel_multiplier(uint8_t instance, uint16_t mul)
{
    _imu_p->_accel_raw_sampling_multiplier[instance] = mul;
}

void sensor_imu_backend_set_raw_sample_gyro_multiplier(uint8_t instance, uint16_t mul)
{
    _imu_p->_gyro_raw_sampling_multiplier[instance] = mul;
}

// called by subclass when data is received from the sensor, thus
// at the 'sensor rate'
void sensor_imu_backend_notify_new_accel_sensor_rate_sample(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *accel)
{

}
void sensor_imu_backend_notify_new_gyro_sensor_rate_sample(sensor_imu_backend *imu_backend, uint8_t instance, const Vector3f_t *gyro)
{

}

/*------------------------------------test------------------------------------*/


